#include "PathSearch.h"
#include <iostream>

namespace fullsail_ai { namespace algorithms {

	template <typename T>
	static bool CompareHCost(T const& _nodeA, T const& _nodeB);

	PathSearch::PathSearch() : nodeList(CompareHCost)
	{		
	}

	PathSearch::~PathSearch()
	{
		this->exit();
	}

	void PathSearch::initialize(TileMap* _tileMap)
	{
		tileMap = _tileMap;
	}

	void PathSearch::enter(int startRow, int startColumn, int goalRow, int goalColumn)
	{
		searchDone = false;

		// Create root node at start position
		startTile = tileMap->getTile(startRow, startColumn);
		goalTile = tileMap->getTile(goalRow, goalColumn);

		// Temporary hCost of 0 for testing
		float startDistance = Distance(startTile, goalTile);
		Node* rootNode = new Node(NULL, startTile, 0.0f, startDistance, startDistance);

		nodeList.push(rootNode);
		createdMap[startTile] = rootNode;
	}
	//****************************************
	/* Path Algorithm: A* algorithm */
	//****************************************
	void PathSearch::update(long timeslice)
	{
		while (!nodeList.empty())
		{
#ifdef _DRAWING_
			tileMap->resetTileDrawing();

			for (std::map<Tile*,Node*>::iterator iter = createdMap.begin(); iter != createdMap.end(); ++iter)
			{
				(*iter).first->setFill(0xFF0000FF);
			}
#endif
			// TRAVERSE!
			Node const* currNode = nodeList.front();
			nodeList.pop();

			if (currNode->state == goalTile)
			{
				searchDone = true;

				// Build solution list
				while (currNode)
				{
					solutionVec.push_back(currNode->state);

//#ifdef _DRAWING_
					if (currNode->state == startTile)
						currNode->state->setLineTo(startTile, 0xFFFF0000);
					else
						currNode->state->setLineTo(currNode->parent->state, 0xFFFF0000);
//#endif
					currNode = currNode->parent;
				}

#ifdef _DEBUG_
			std::cout << "Vector Size: " << solutionVec.size() << '\n';
#endif
				return;
			}

			// Fill the adjacent node array for processing
			GetAdjacentTiles(currNode->state);

			for (unsigned int i = 0; i < NUM_OPERATORS; i += 2)
			{
				// 1st iteration
				if (adjTiles[i] != NULL && adjTiles[i]->getWeight() != 0)
				{
					float tempGCost = currNode->givenCost + Distance(currNode->state, adjTiles[i]) * adjTiles[i]->getWeight();

					if (!createdMap[adjTiles[i]])
					{
						float tempHCost = Distance(adjTiles[i], goalTile);
						createdMap[adjTiles[i]] = new Node(currNode, adjTiles[i], tempGCost, tempHCost, (tempGCost + tempHCost * 1.1f));
						nodeList.push(createdMap[adjTiles[i]]);
#ifdef _DRAWING_
						createdMap[adjTiles[i]]->state->setFill(0xFF00FF00);
						createdMap[adjTiles[i]]->state->setOutline(0xFFFF0000);
#endif
					}
					else
					{
						if (createdMap[adjTiles[i]]->givenCost > tempGCost)
						{
							nodeList.remove(createdMap[adjTiles[i]]);
							createdMap[adjTiles[i]]->givenCost = tempGCost;
							createdMap[adjTiles[i]]->parent = currNode;
							createdMap[adjTiles[i]]->finalCost = createdMap[adjTiles[i]]->givenCost + createdMap[adjTiles[i]]->hCost * 1.1f;
							nodeList.push(createdMap[adjTiles[i]]);
						}
					}
				}

				// 2nd iteration
				if (adjTiles[i+1] != NULL && adjTiles[i+1]->getWeight() != 0)
				{
					float tempGCost = currNode->givenCost + Distance(currNode->state, adjTiles[i+1]) * adjTiles[i+1]->getWeight();

					if (!createdMap[adjTiles[i+1]])
					{
						float tempHCost = Distance(adjTiles[i+1], goalTile);
						createdMap[adjTiles[i+1]] = new Node(currNode, adjTiles[i+1], tempGCost, tempHCost, (tempGCost + tempHCost * 1.1f));
						nodeList.push(createdMap[adjTiles[i+1]]);
#ifdef _DRAWING_
						createdMap[adjTiles[i+1]]->state->setFill(0xFF00FF00);
						createdMap[adjTiles[i+1]]->state->setOutline(0xFFFF0000);
#endif
					}
					else
					{
						if (createdMap[adjTiles[i+1]]->givenCost > tempGCost)
						{
							nodeList.remove(createdMap[adjTiles[i+1]]);
							createdMap[adjTiles[i+1]]->givenCost = tempGCost;
							createdMap[adjTiles[i+1]]->parent = currNode;
							createdMap[adjTiles[i+1]]->finalCost = createdMap[adjTiles[i+1]]->givenCost + createdMap[adjTiles[i+1]]->hCost * 1.1f;
							nodeList.push(createdMap[adjTiles[i+1]]);
						}
					}
				}
			}

#ifdef _DEBUG_
			std::cout << "***\nGiven Cost: " << currNode->givenCost << '\n';
			std::cout << "H Cost: " << currNode->hCost << '\n';
			std::cout << "Final Cost: " << currNode->finalCost << "\n***\n";
#endif
#ifdef _DRAWING_
			while (currNode)
			{
				if (currNode->state == startTile)
				{
					currNode->state->setLineTo(startTile, 0xFFFF0000);
					break;
				}
				else
				{
					currNode->state->setLineTo(currNode->parent->state, 0xFFFF0000);
					currNode = currNode->parent;
				}
			}
#endif
			// Exit if stepping through
			if (timeslice == 0)
				return;
		}
	}

	void PathSearch::exit()
	{
		for (std::map<Tile*,Node*>::iterator iter = createdMap.begin(); iter != createdMap.end(); ++iter)
		{
			// Clean the node
			Node* deleteNode = (*iter).second;
			delete deleteNode;
			deleteNode = NULL;
		}
		createdMap.clear();

		nodeList.clear();

		solutionVec.clear();
	}

	void PathSearch::shutdown()
	{
		this->exit();
	}

	bool PathSearch::isDone() const
	{
		return searchDone;
	}

	std::vector<Tile const*> const PathSearch::getSolution() const
	{
		return solutionVec;
	}

	void PathSearch::GetAdjacentTiles(Tile const* _currTile)
	{
		int currTileRow = _currTile->getRow();
		int currTileColumn = _currTile->getColumn();

		adjTiles[0] = tileMap->getTile(currTileRow-1, currTileColumn);	// Up
		adjTiles[1] = tileMap->getTile(currTileRow, currTileColumn+1);	// Right
		adjTiles[2] = tileMap->getTile(currTileRow+1, currTileColumn);	// Down
		adjTiles[3] = tileMap->getTile(currTileRow, currTileColumn-1);	// Left

		if (_currTile->getRow() & 1)	// Odd - go right
		{
			adjTiles[4] = tileMap->getTile(currTileRow-1, currTileColumn+1);	// Upper Diag
			adjTiles[5] = tileMap->getTile(currTileRow+1, currTileColumn+1);	// Lower Diag
		}
		else	// Even - go left
		{
			adjTiles[4] = tileMap->getTile(currTileRow-1, currTileColumn-1);	// Upper Diag
			adjTiles[5] = tileMap->getTile(currTileRow+1, currTileColumn-1);	// Lower Diag
		}
	}

	// Compare function for Priority Queue
	template <typename T>
	bool CompareHCost(T const& _nodeA, T const& _nodeB)
	{
		return ((Node*)_nodeA)->finalCost > ((Node*)_nodeB)->finalCost;
	}

	// Distance between 2 points
	float PathSearch::Distance(Tile* _currTile, Tile* _goalTile)
	{
		float x = (float)((_goalTile->getXCoordinate() - _currTile->getXCoordinate()));
		x *= x;
		float y = (float)((_goalTile->getYCoordinate() - _currTile->getYCoordinate()));
		y *= y;

		return sqrt(x + y);
	}

}}  // namespace fullsail_ai::algorithms

